/** 
    @file physics.cpp
    @brief Game physics
*/
#include "physics.hpp"

template<> pm3::PHYSICS* Ogre::Singleton<pm3::PHYSICS>::ms_Singleton = 0;
pm3::PHYSICS pm3::g_physics;

ContactAddedCallback pm3::g_contact_added_callback;

pm3::PHYSICS::PHYSICS() {
    m_broadphase = NULL;
    m_collision_cfg = NULL;
    m_dispatcher = NULL;
    m_solver = NULL;

    m_physics_world = NULL;
    m_physics_debug = NULL;
}

pm3::PHYSICS::~PHYSICS() {
    // Free Bullet stuff..
    delete m_solver;
    delete m_dispatcher;
    delete m_collision_cfg;
    delete m_broadphase;
}

void pm3::PHYSICS::init() {
    static bool times = false;
    LOGGER *log = LOGGER::getSingletonPtr();
    pm3::INTERFACE *intf = pm3::INTERFACE::getSingletonPtr();

    if (!intf)
        return;

    if (!intf->get_scenemgr()) {
        if (log)
            log->report( "Physics", "E: Scene manager must be initialized before physics\n" );
        return;
    }

    // Make sure it's only initialized once
    if (!times)
        times = true;
    else
        return;

    if (log)
        log->report( "Physics", "I: Initializing Bullet Physics\n" );

    // Initialize bullet
    m_broadphase = new btAxisSweep3( btVector3( -10000, -10000, -10000 ), btVector3( 10000, 10000, 10000 ), 1024 );
    m_collision_cfg = new btDefaultCollisionConfiguration();

    m_collision_cfg->setConvexConvexMultipointIterations(10,5);

    m_dispatcher = new btCollisionDispatcher( m_collision_cfg );
    m_solver = new btSequentialImpulseConstraintSolver();

    m_physics_world = new btDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collision_cfg );

	m_physics_world->getSolverInfo().m_splitImpulse = true;
	m_physics_world->getSolverInfo().m_splitImpulsePenetrationThreshold = 1e30f;
	m_physics_world->getSolverInfo().m_maxErrorReduction = 1e30f;
	m_physics_world->getSolverInfo().m_erp  =1.f;
	m_physics_world->getSolverInfo().m_erp2 = 1.f;

    //! \todo It should be possible to override gravity from mapscripts
    m_physics_world->setGravity( btVector3( 0, -10, 0 ) );
//    m_physics_world->setGravity( btVector3( 0, 0, 0 ) );

    if (log)
        log->report( "Physics", "I: Initializing debug drawer\n" );

    // Set up physics debug drawer
    m_physics_debug = new BtOgre::DebugDrawer( intf->get_scenemgr()->getRootSceneNode(), m_physics_world );
    m_physics_world->setDebugDrawer( m_physics_debug );

    // Set contact callback
    g_contact_added_callback = contact_added_callback;
}

static bool pm3::contact_added_callback( 
    btManifoldPoint& cp, const btCollisionObject* colObj0, 
    int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1 )
{

    // Find the triangle mesh object and the other object
    const btCollisionObject *trimesh = colObj0, *other = colObj1;
    int trimesh_pid = partId0, other_pid = partId1;
    int trimesh_id = index0, other_id = index1;
    // Make sure we're giving btAdjustInternalEdgeContacts the right arguments 
    if ( colObj1->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE ) {
        trimesh = colObj1;
        trimesh_pid = partId1;
        trimesh_id = index1;

        other = colObj0;
        other_pid = partId0;
        other_id = index0;
    }

//    if (enable) {
        btAdjustInternalEdgeContacts( cp, trimesh, other, trimesh_pid, trimesh_id );
//        btAdjustInternalEdgeContacts( cp, other, trimesh, other_pid, other_id );
//    }

    return false;
}

